#include "arpsdcalculator.h"
#include <cmath>
#include <unsupported/Eigen/Splines>
#include "qdebug.h"

ARPSDCalculator::ARPSDCalculator(QObject *parent) : QObject(parent)
{
    // 构造函数
}
// 样条插值函数
QVector<double> ARPSDCalculator::interp1(const QVector<double> &t, const QVector<double> &y, const QVector<double> &t2, const QString &method) {
    if (method != "spline") {
        return QVector<double>();
    }

    int n = t.size();
    QVector<double> h(n - 1), b(n - 1), u(n), v(n), z(n);

    // 计算h和b
    for (int i = 0; i < n - 1; ++i) {
        h[i] = t[i + 1] - t[i];
        b[i] = (y[i + 1] - y[i]) / h[i];
    }

    // 计算u和v
    u[1] = 2 * (h[0] + h[1]);
    v[1] = 6 * (b[1] - b[0]);
    for (int i = 2; i < n - 1; ++i) {
        u[i] = 2 * (h[i - 1] + h[i]) - (h[i - 1] * h[i - 1]) / u[i - 1];
        v[i] = 6 * (b[i] - b[i - 1]) - (h[i - 1] * v[i - 1]) / u[i - 1];
    }

    // 计算z（二阶导数）
    z[n - 1] = 0;
    for (int i = n - 2; i > 0; --i) {
        z[i] = (v[i] - h[i] * z[i + 1]) / u[i];
    }
    z[0] = 0;

    // 计算插值结果
    QVector<double> y2;
    for (double t_val : t2) {
        int i = 0;
        while (i < n - 1 && t_val > t[i + 1]) {
            ++i;
        }
        double dx = t_val - t[i];
        y2.push_back(y[i]  + b[i] * dx + (z[i + 1] - z[i]) / (6 * h[i]) * dx * dx * dx - z[i] / 2 * dx * dx);
    }

    return y2;
}
// 插值函数
Eigen::VectorXd ARPSDCalculator::interpolate(const Eigen::VectorXd& t, const Eigen::VectorXd& y, double fs) {
    double t_start = t(0);
    double t_end = t(t.size()  - 1);
    Eigen::VectorXd t2 = Eigen::VectorXd::LinSpaced((t_end - t_start) * fs + 1, t_start, t_end);
    qDebug()<<"t2="<<eigenToQVector(t2);
    // 样条插值
    Eigen::Spline<double, 1> spline = Eigen::SplineFitting<Eigen::Spline<double, 1>>::Interpolate(y.transpose(),  3, t);
    Eigen::VectorXd y_interp(t2.size());
    for (int i = 0; i < t2.size();  ++i) {
        y_interp(i) = spline(t2(i))(0);
    }
    return y_interp;
}

// 计算均值
double ARPSDCalculator::mean(const Eigen::VectorXd& vec) {
    return vec.mean();
}

// 应用汉明窗
void ARPSDCalculator::applyHammingWindow(Eigen::VectorXd& vec) {
    int N = vec.size();
    for (int i = 0; i < N; ++i) {
        vec(i) *= 0.54 - 0.46 * cos(2 * M_PI * i / (N - 1));
    }
}

// 计算 AR 模型参数（Burg 方法）
void ARPSDCalculator::arburg(const Eigen::VectorXd& y, int AR_order, Eigen::VectorXd& A, double& variance) {
    // 简化实现，实际需要更复杂的算法
    A = Eigen::VectorXd::Zero(AR_order + 1);
    A(0) = 1.0;
    variance = y.array().square().mean();
}

// 计算频率响应
void ARPSDCalculator::freqz(const Eigen::VectorXd& b, const Eigen::VectorXd& a, int nfft, double fs, Eigen::VectorXd& H, Eigen::VectorXd& F) {
    H = Eigen::VectorXd::Zero(nfft);
    F = Eigen::VectorXd::LinSpaced(nfft, 0, fs / 2);

    for (int i = 0; i < nfft; ++i) {
        std::complex<double> h(0, 0);
        for (int j = 0; j < b.size();  ++j) {
            h += b(j) * std::exp(std::complex<double>(0, -2 * M_PI * F(i) * j / fs));
        }
        H(i) = std::abs(h);
    }
}

// 计算 AR 模型 PSD
QPair<QVector<double>, QVector<double>> ARPSDCalculator::calculate(const Eigen::VectorXd& t, const Eigen::VectorXd& y, double fs, int nfft, int AR_order) {
    // 插值
    Eigen::VectorXd y_interp = interpolate(t, y, fs);
    qDebug()<<eigenToQVector(y_interp);
    // 去除均值
    qDebug()<<mean(y_interp);
    y_interp.array()  -= mean(y_interp);

    // 加汉明窗
    applyHammingWindow(y_interp);
    QVector<double> dd=eigenToQVector(y_interp);
    qDebug()<<dd;
    qDebug()<<dd.size();
    // 计算 AR 模型参数
    Eigen::VectorXd A;
    double variance;
    arburg(y_interp, AR_order, A, variance);

    // 计算频率响应
    Eigen::VectorXd H, F;
    freqz(Eigen::VectorXd::Ones(1), A, nfft, fs, H, F);

    // 计算 PSD
    Eigen::VectorXd PSD = H.array().square()  * (variance / fs);

    // 转换为 QVector
    QVector<double> PSD_qvec = eigenToQVector(PSD);
    QVector<double> F_qvec = eigenToQVector(F);
    return qMakePair(PSD_qvec, F_qvec);
}


QVector<double> ARPSDCalculator::eigenToQVector(const Eigen::VectorXd& eigenVec) {
    QVector<double> qvec(eigenVec.size());
    for (int i = 0; i < eigenVec.size();  ++i) {
        qvec[i] = eigenVec(i);
    }
    return qvec;
}
